#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <sdf/sdf.hh>

namespace gazebo
{
  class ModelPush : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
      std::cout << "modelpush plugin is loading" << std::endl;
      // Store the pointer to the model
      this->model = _parent;

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
          std::bind(&ModelPush::OnUpdate, this));

      _sdf = _sdf->GetFirstElement();
      
      do{
        std::string name = _sdf->GetName();
        if (name == "velX")
            _sdf->GetValue()->Get(velX_);
        else if (name == "velY")
            _sdf->GetValue()->Get(velY_);
        else if (name == "velZ")
            _sdf->GetValue()->Get(velZ_);
        else if (name == "velRoll")
            _sdf->GetValue()->Get(velRoll_);
        else if (name == "velPitch")
            _sdf->GetValue()->Get(velPitch_);
        else if (name == "velYaw")
            _sdf->GetValue()->Get(velYaw_);
        else 
            throw std::runtime_error("Invalid parameter for model_push_Plugin");
        
        _sdf = _sdf->GetNextElement();
      }while(_sdf);

      std::cout
      << std::endl
      << "model_push_Plugin: The model_push plugin is attached to model "
      << model->GetName() << std::endl;
    }

    // Called by the world update start event
    public: void OnUpdate()
    {
      // Apply a small linear velocity to the model.
      this->model->SetLinearVel(ignition::math::Vector3d(velX_, velY_, velZ_));
      // Apply a small angular velocity to the model.
      this->model->SetAngularVel(ignition::math::Vector3d(velRoll_, velPitch_, velYaw_));
    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;
    
    // Linear vel
    protected: double velX_;
    protected: double velY_;
    protected: double velZ_;

    // Angular vel
    protected: double velRoll_;
    protected: double velPitch_;
    protected: double velYaw_;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(ModelPush)
}